
.pushsection    ".head.text","awx"

_head:
    b   stext

// 启动入口
.global stext; .align 2; stext:
    bl	el2_setup
    bl	__create_page_tables
    bl	__cpu_setup
    b	__primary_switch
.type stext, @function; .size stext, .-stext

// 配置 el2, 并且切换回 el1
.global el2_setup; .align 2; el2_setup:
    msr	SPsel, #1       // 使用 SP_EL(1, 2)
    mrs x0, CurrentEL
    cmp x0, #CurrentEL_EL2
    b.eq	1f
    mov_q	x0, (SCTLR_EL1_RES1 | ENDIAN_SET_EL1)
    msr	sctlr_el1, x0
    mov	w0, #BOOT_CPU_MODE_EL1
    isb
    ret

1:  mov_q	x0, (SCTLR_EL2_RES1 | ENDIAN_SET_EL2)
    msr	sctlr_el2, x0

    mov_q	x0, (SCTLR_EL1_RES1 | ENDIAN_SET_EL1)
    msr	sctlr_el1, x0

    /* spsr */
    mov	x0, #(PSR_F_BIT | PSR_I_BIT | PSR_A_BIT | PSR_D_BIT | PSR_MODE_EL1h)
    msr	spsr_el2, x0
    msr	elr_el2, lr
    mov	w0, #BOOT_CPU_MODE_EL2		// This CPU booted in EL2
    eret
.type el2_setup, @function; .size el2_setup, .-el2_setup

// 创建页表
__create_page_tables:
    mov	x28, lr

    // 无效化 swapper_pg_dir dcache
    adrp    x0, swapper_pg_dir
    adrp    x1, swapper_pg_end
    sub x1, x1, x0
    bl	__inval_dcache_area

    // 清空 swapper_pg_dir
    adrp	x0, swapper_pg_dir
    adrp	x1, swapper_pg_end
    sub	x1, x1, x0
1:	stp	xzr, xzr, [x0], #16
    stp	xzr, xzr, [x0], #16
    stp	xzr, xzr, [x0], #16
    stp	xzr, xzr, [x0], #16
    subs	x1, x1, #64
    b.ne	1b

    // 映射 idmap
    adrp    x0, idmap_pg_dir    // pgd 页表物理地址
    adrp    x3, _stext_idmap    // 物理起始地址
    adr_l   x6, _etext_idmap    // 物理结束地址
    mov	x7, SWAPPER_MM_NORMALFLAGS  // 映射属性

    map_memory x0, x1, x3, x6, x7, x3, x4, x10, x11, x12, x13, x14

    // 映射内核地址
    adrp    x0, swapper_pg_dir  // pgd 页表物理地址
    adrp	x3, _text           // 物理起始地址
    ldr     x5, =_text          // 虚拟地址
    adrp    x6, _end            // 物理结束地址
    sub     x6, x6, x3          // _end - _text
    add     x6, x6, x5          // 虚拟结束地址

    map_memory x0, x1, x5, x6, x7, x3, x4, x10, x11, x12, x13, x14

    // 映射线性地址
    adrp    x0, swapper_pg_dir  // pgd 页表物理地址
    mov	    x3, #0              // 物理起始地址
    mov_q   x5, KMEM_VADDR      // 虚拟地址
    mov_q   x6, KMEM_VADDR_END  // 物理结束地址

    map_memory x0, x1, x5, x6, x7, x3, x4, x10, x11, x12, x13, x14

    // 内核页地址
    adrp    x0, swapper_pg_dir   // pgd 页表物理地址
    mov	    x3, #0               // 物理起始地址
    mov_q   x5, KPAGE_VADDR      // 虚拟地址
    mov_q   x6, KPAGE_VADDR_END  // 物理结束地址

    map_memory x0, x1, x5, x6, x7, x3, x4, x10, x11, x12, x13, x14

    // 映射 io 地址
    adrp    x0, swapper_pg_dir  // pgd 页表物理地址
    mov	    x3, #0              // 物理起始地址
    mov_q   x5, KIO_VADDR       // 虚拟地址
    mov_q   x6, KIO_VADDR_END   // 物理结束地址
    mov	x7, SWAPPER_MM_IOFLAGS

    map_memory x0, x1, x5, x6, x7, x3, x4, x10, x11, x12, x13, x14

    adrp	x0, idmap_pg_dir
    adrp	x1, swapper_pg_end
    sub	x1, x1, x0
    dmb	sy
    bl	__inval_dcache_area

    ret	x28
.type __create_page_tables, @function; .size __create_page_tables, .-__create_page_tables
    .ltorg

.global __cpu_setup; .align 2; __cpu_setup:
    tlbi	vmalle1				// Invalidate local TLB
    dsb	nsh

    mov	x0, #3 << 20
    msr	cpacr_el1, x0			// Enable FP/ASIMD
    mov	x0, #1 << 12			// Reset mdscr_el1 and disable
    msr	mdscr_el1, x0			// access to the DCC from EL0
    isb					// Unmask debug exceptions now,
    enable_dbg				// since this is per-cpu
    reset_pmuserenr_el0 x0			// Disable PMU access from EL0

    ldr x5, =MAIR_ATTR
    msr	mair_el1, x5

    mov_q x0, SCTLR_EL1_SET
    ldr x10, =(TCR_TxSZ | TCR_CACHE_FLAGS | TCR_SMP_FLAGS | TCR_TG_FLAGS | TCR_ASID16 | TCR_TBI0 | TCR_A1)

    mov_q x9, TCR_T0SZ
    tcr_set_t0sz	x10, x9

    // Set the IPS bits in TCR_EL1
    tcr_compute_pa_size x10, #TCR_IPS_SHIFT, x5, x6
    msr	tcr_el1, x10
    ret
.type __cpu_setup, @function; .size __cpu_setup, .-__cpu_setup

__primary_switched:
    adrp    x4, _stack_top
    mov     sp, x4
    adr_l   x5, INIT_TASK
    msr sp_el0, x5

    adr_l   x8, vectors
    msr	vbar_el1, x8
    isb

    // Clear BSS
    adr_l   x0, _sbss
    adr_l   x1, _ebss
    subs x1, x1, x0
    b.eq    2f
    // 清零单位 64 字节
1:	stp	xzr, xzr, [x0], #16
    stp	xzr, xzr, [x0], #16
    stp	xzr, xzr, [x0], #16
    stp	xzr, xzr, [x0], #16
    subs	x1, x1, #64
    b.ne	1b
2:
    dsb	ishst

    mov	x29, #0
    mov	x30, #0
    b	__ffi_start_kernel
.type __primary_switched, @function; .size __primary_switched, .-__primary_switched

__primary_switch:
    adrp	x1, swapper_pg_dir
    bl	__enable_mmu
    ldr	x8, =__primary_switched
    adrp	x0, _text
    br	x8
.type __primary_switch, @function; .size __primary_switch, .-__primary_switch

.global __enable_mmu; .align 2; __enable_mmu:
    mrs x2, ID_AA64MMFR0_EL1
    ubfx x2, x2, #ID_AA64MMFR0_TGRAN_SHIFT, 4
    cmp x2, #ID_AA64MMFR0_TGRAN_SUPPORTED
    b.ne __no_granule_support
    adrp x2, idmap_pg_dir
    phys_to_ttbr x1, x1
    phys_to_ttbr x2, x2
    msr ttbr0_el1, x2
    msr ttbr1_el1, x1
    isb
    msr sctlr_el1, x0
    isb
    ic	iallu
    dsb	nsh
    isb
    ret
.type __enable_mmu, @function; .size __enable_mmu, .-__enable_mmu

__no_granule_support:
1:
    wfe
    wfi
    b 1b
.type __no_granule_support, @function ; .size __no_granule_support, .-__no_granule_support

.popsection
